/*
 * @Description: 显示模块具体实现
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:06:25
 * @LastEditTime: 2021-11-30 21:06:01
 * @LastEditors: Sang Hao
 */
#ifndef LIDAR_SLAM_MAPPING_VIEWER_VIEWER_HPP_
#define LIDAR_SLAM_MAPPING_VIEWER_VIEWER_HPP_

#include <string>
#include <Eigen/Dense>
#include <deque>
#include <yaml-cpp/yaml.h>

#include "lidar_slam/models/segmentation/cloud_segment_interface.hpp"
#include "lidar_slam/models/cloud_filter/cloud_filter_interface.hpp"
#include "lidar_slam/sensor_data/pose_data.hpp"
#include "lidar_slam/sensor_data/cloud_data.hpp"
#include "lidar_slam/sensor_data/key_frame.hpp"

namespace lidar_slam {
class Viewer {
private:
	/* data */
	std::string data_path_ = "";
	int local_frame_num_ = 20;
	
	std::string key_frames_path_ = "";
	std::string map_path_ = "";

	std::shared_ptr<CloudFilterInterface> frame_filter_ptr_;
	std::shared_ptr<CloudFilterInterface> local_map_filter_ptr_;
	std::shared_ptr<CloudFilterInterface> global_map_filter_ptr_;
	std::shared_ptr<CloudFilterInterface> multi_plane_clipper_filter_ptr_;
	std::shared_ptr<CloudSegmentInterface> global_map_segment_ptr_;

	Eigen::Matrix4f	pose_to_optimize_ = Eigen::Matrix4f::Identity();
	PoseData optimized_odom_;
	CloudData optimized_cloud_;
	std::deque<KeyFrame> optimized_key_frames_;
	std::deque<KeyFrame> all_key_frames_;

	bool has_new_global_map_ = false;
	bool has_new_local_map_ = false;
	bool need_multi_plane_clipper_filter_ = false;

private:
	bool InitWithConfig();
	bool InitParam(const YAML::Node& config_node);
	bool InitDataPath(const YAML::Node& config_node);
	bool InitFilter(std::string filter_user,
					std::shared_ptr<CloudFilterInterface>& filter_ptr,
					const YAML::Node& config_node);
	bool InitSegment(std::string segment_user,
					 std::shared_ptr<CloudSegmentInterface>& segment_ptr,
					 const YAML::Node& config_node);
	bool OptimizeKeyFrames();
	bool JointGlobalMap(CloudData::CLOUD_PTR& global_map_ptr);
	bool JointLocalMap(CloudData::CLOUD_PTR& local_map_ptr);
	bool JointCloudMap(const std::deque<KeyFrame>& key_frames,
							CloudData::CLOUD_PTR& map_cloud_ptr);
public:
	Viewer();
	
	bool UpdateWithOptimizedKeyFrames(std::deque<KeyFrame>& optimized_key_frames);
	bool UpdateWithNewKeyFrame(std::deque<KeyFrame>& new_key_frames,
								PoseData transformed_data,
								CloudData cloud_data);
	bool SaveMap();
	bool SegMap(const CloudData::CLOUD_PTR& input_cloud_ptr, 
		CloudRGBData::CLOUD_PTR& seged_cloud_ptr);
	Eigen::Matrix4f& GetCurrentPose();
	CloudData::CLOUD_PTR& GetCurrentScan();
	bool GetLocalMap(CloudData::CLOUD_PTR& local_map_ptr);
	bool GetGlobalMap(CloudData::CLOUD_PTR& global_map_ptr);
	bool HasNewLocalMap();
	bool HasNewGlobalMap();

};
}

#endif